package robotninja.asctec.test.movementcommands.commands;

import java.util.LinkedList;
import java.util.Queue;

import robotninja.driver.asctec.AsctecDriver;

/**
 * Class to send timed commands with the driver.asctec interface
 */
public class CommandSender {

	static public enum COMMAND_TYPE {
		ROLL(1), PITCH(2), YAW(3), THRUST(4);

		private int position;

		private COMMAND_TYPE(int c) {
			this.position = c;
		}

		public int getValue() {
			return position;
		}
	}

	private Queue<Command> commandQueue = null;
	private CommunicationThread com;

	// TODO: Maybe add a command object?

	public CommandSender(AsctecDriver driver) {
		this.commandQueue = new LinkedList<Command>();

		this.com = new CommunicationThread(driver);
	}

	public CommandSender addCommand(int waitMiliSecs, double pitch,
			double roll, double yaw, int value) {

		// Validate that the command is right.
		// if (!isValidCommand(cType, value) || waitMiliSecs < 0) {
		// throw new InvalidParameterException(
		// "Invalid value for command type " + cType.toString());
		// }

		commandQueue.add(new Command(waitMiliSecs, pitch, roll, yaw, value));

		return this;
	}

	private boolean isValidCommand(COMMAND_TYPE cType, int value) {
		// TODO: Validate command
		return true;
	}

	public void run() {
		com.start();
	}

	public class CommunicationThread extends Thread {

		private AsctecDriver driver;

		private int sleepTime = 80;

		/**
		 * 
		 * @param driver
		 */
		public CommunicationThread(AsctecDriver driver) {
			this.driver = driver;
		}

		@Override
		public void run() {
			Command cmd = commandQueue.poll();
			
			
			while (cmd != null) {
				// Send command
				driver.pryControl(cmd.thrust, cmd.pitch, cmd.roll, cmd.yaw);

				// Sleep
				try {
					Thread.sleep(sleepTime);
				} catch (InterruptedException e) {
					e.printStackTrace();
				}
				
				cmd.time -= sleepTime;
				if(cmd.time < 0) cmd = commandQueue.poll();
			}
			driver.pryControl(0, 0, 0, 0);
			driver.turnOff();
			
		}
	}

}
